
import utm
import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
import gpsAndUtm
import os

init_point = [121.54250, 30.412350]

# L path generate
l_1 = 150
alpha = 135
l_2 = 60
l_3 = 80

L_path = []
east, north, zone_number, zone_letter = gpsAndUtm.get_utm(init_point[0], init_point[1])
L_path.append([east, north])

east += l_1

L_path.append([east , north])

east += l_2 * np.cos((alpha-90)*np.pi/180.0)
north -= l_2 * np.sin((alpha-90)*np.pi/180.0)

L_path.append([east, north])

north -= l_3

L_path.append([east, north])

east -= l_2 * np.cos((alpha-90) * np.pi/180.0)
north -= l_2 * np.sin((alpha-90) * np.pi/180.0)
L_path.append([east, north])

east -= l_1
L_path.append([east, north])

east -= l_2 * np.cos((alpha-90) * np.pi/180.0)
north -= l_2 * np.sin((alpha-90) * np.pi/180.0)
L_path.append([east, north])

north -= l_3
L_path.append([east, north])

east += l_2* np.cos((alpha-90) * np.pi/180.0)
north -= l_2 * np.sin((alpha-90) * np.pi /180.0)
L_path.append([east, north])

east += l_1
L_path.append([east, north])

print(L_path)

L_x = []
L_y = []
L_lon = []
L_lat = []
for point in L_path:
    L_x.append(point[0])
    L_y.append(point[1])

    lon, lat = gpsAndUtm.get_wgs84(point[0], point[1], zone_number, zone_letter)
    L_lon.append(lon)
    L_lat.append(lat)
    
# convert to pandas, save to csv
HOME = os.getcwd()
gps_path = pd.DataFrame()
gps_path['lon'] = L_lon
gps_path['lat'] = L_lat
gps_path.to_csv(f'{HOME}/usv_simulator/navigation/gps_S_path.csv')

plt.plot(L_x, L_y)
plt.show()
